#include "GridRunner.h"

GridRunner::GridRunner(){}

GridRunner::~GridRunner(void){}

void GridRunner::run(GridNode& start, GridNode& goal){
	// Create an open list
	openCapacity = GridRunner::DEFAULT_OPEN_CAPACITY;
	openlist = new GridNode*[openCapacity];
	openSize = 0;

	closedCapacity = GridRunner::DEFAULT_CLOSED_CAPACITY;
	closedlist = new GridNode*[closedCapacity];
	closedSize = 0;

	goalX=pGoalNode->x;
	goalY=pGoalNode->y;
	startX=pStartNode->x;
	startY=pStartNode->y;

	GridNode* current = &start;	
	bool goalFound = false;
	insertToOpen(&start,h(start));
	while(openSize>0){
		current = popOpen();
		if (isGoal(current)){
			goalFound=true;
			break;
		}
		expand(current);
		printf("Expanded node %d\n",current);
	}
	printf("Hey Hey\n");

	// Free memory
	delete[] openlist;
	delete[] closedlist;
}

int GridRunner::h(GridNode& pNode){
	return -1;
}

GridNode* GridRunner::move(GridNode* pNode, int pOpIndex){
	return NULL;
}

bool GridRunner::isGoal(GridNode* pNode){
	return true;
}

/**
 * Pop the top of the openlist
 **/
GridNode* GridRunner::popOpen(){
	GridNode* top = openlist[0];
	--openSize;
	
	if(openSize>0){
		openlist[0]=openlist[openSize]; // Put last item in place of top node
		heapifyDown(0);
	}
	return top;
}

/**
 * Propogate down the node at heap index down the heap until the heap rules are preserved.
 * Returns the new index of the node.
 **/
int GridRunner::heapifyDown(int pHeapIndex){
	int current = pHeapIndex;
	int child1;
	int child2;
	do{
		child1 = childLeft(current);
		if(openSize>child1){
			if(testAndSwapOpen(current, child1)){
				current=child1;
				continue;
			}
		}
		child2 = childRight(current);
		if(openSize>child2){
			if(testAndSwapOpen(current, child1)){
				current=child1;
				continue;
			}
		}
		// No swapping has been performed
		break;
	}while(true);
	return current;
}


/** 
 * Propogate the given index until it find a position that is valid according to heap rules
 * Returns the new index of the node.
 **/
int GridRunner::heapifyUp(int pHeapIndex){
	int parentIndex;
	int current = pHeapIndex;
	while(current!=0){		
		parentIndex = parent(current);
		if(testAndSwapOpen(parentIndex,current)){
			current=parentIndex;
		}
		else{
			break;
		}
	}
	return current;
}

/**
 * Test if the parent has a higher value than the child. 
   If so - replace them.
   Returns true if a swap has occured, or false otherwise.
  **/
bool GridRunner::testAndSwapOpen(int pParent,int pChild){
	if(openlist[pParent]->f>openlist[pChild]->f){
		openlist[pParent]->index = pChild;
		openlist[pChild]->index = pParent;

		GridNode* temp = openlist[pParent];
		openlist[pParent]=openlist[pChild];
		openlist[pChild]=temp;		
		return true;
	}
	return false;
}

/** 
 * Insert a node to the openlist
 **/
void GridRunner::insertToOpen(GridNode* node, int f){
	node->f=f;

	// If open is full - enlarge capacity
	if(openSize==openCapacity){ 
		int newCapacity = openCapacity*2;
		GridNode** tempOpen = new GridNode*[newCapacity];

		memcpy(tempOpen,openlist,openCapacity * sizeof(GridNode*));
		delete[] openlist;
		this->openlist = tempOpen;
		this->openCapacity = newCapacity;
	}

	int heapIndex = openSize;	
	openlist[heapIndex]=node;
	node->index=heapIndex;
	openSize = openSize+1;

	heapifyUp(heapIndex);
}

/**
 * Insert a node into the closed list.
 **/
void GridRunner::insertToClosed(GridNode* pNode){
	closedSize++;
	// If closed is full - enlarge capacity
	if(closedSize==closedCapacity){ 
		int newCapacity = closedCapacity*2;
		GridNode** tempClosed = new GridNode*[newCapacity];

		memcpy(tempClosed,closedlist,closedCapacity * sizeof(GridNode*));
		delete[] closedlist;
		closedlist = tempClosed;
		closedCapacity = newCapacity;
	}
	closedlist[closedSize-1]=pNode;
}

/**
 * Remove the node at the given index from closed
 **/
void GridRunner::removeFromClosed(int pIndex){
	if(pIndex<closedSize-1){
		closedlist[pIndex]=closedlist[closedSize-1];
	}
	closedSize--;
}

/**
 * Remove a node from the heuristic table
 **/
void GridRunner::expand(GridNode* pTopNode){
	// TODO
	GridNode* child;
	int heapIndex;
	int nodeStatus;

	// Insert node into closed list
	insertToClosed(pTopNode);

	for(int i=0;i<NUM_OF_OPS;i++){
		if(isLegalOp(pTopNode,i)){
			child = move(pTopNode,i); // Move function should set g value as well as h
			nodeStatus = findNodeStatus(child);
			if(nodeStatus==GridRunner::STATUS_NEW){
				insertToOpen(child,child->f);
			}
			else{ // Node is already known
				heapIndex=find(child,nodeStatus);
				if(child->g<openlist[heapIndex]->g){ 
					// Node reopening
					if(nodeStatus==GridRunner::STATUS_OPEN)
						reorder(heapIndex);
					
					else // nodeStatus==GridRunner::STATUS_CLOSED
						reopen(heapIndex);
				}
			}
		}
	}
}

/**
 * Returns the status of a node (new, open or closed)
 **/
int GridRunner::findNodeStatus(GridNode* pNode){
	return GridRunner::STATUS_NEW;
}

/**
 * Returns the index of the node in the openlist/closedlist 
 * according to which the node status.
 **/
int GridRunner::find(GridNode* pNode, int pNodeStatus){
	if(pNodeStatus==GridRunner::STATUS_OPEN){
		for(int i=0;i<openSize;i++)
			if(pNode==openlist[i])
				return i;
	}
	else{ // pNodeStatus==GridNode::STATUS_CLOSED
		for(int i=0;i<closedSize;i++)
			if(pNode==closedlist[i])
				return i;
	}
	return -1; // Not found, Should never get here
}

/** 
 * Remove a node from the closedlist and reinsert it to OPEN
 **/
void GridRunner::reopen(int pNodeIndex){
	GridNode* reopenedNode = closedlist[pNodeIndex];
	removeFromClosed(pNodeIndex);
	insertToOpen(reopenedNode,reopenedNode->f);
}

/**
 * Checks if an op is legal 
 **/
bool GridRunner::isLegalOp(GridNode* pNOde, int pOpIndex){
	return true;
}

/**
 * Remove an item from the openlist
 **/
void GridRunner::removeFromOpen(int pHeapIndex){
	openSize--;
	// If the removed node isn't the last in the heap
	if(pHeapIndex!=openSize){
		GridNode* temp = openlist[openSize];	
		openlist[pHeapIndex]=temp; // Put last node in place of removed node
		reorder(pHeapIndex);
	}
}


/**
 * Reinsert the node into the openlist
 **/
void GridRunner::reorder(int pCurrentIndex){
	int newIndex;
	newIndex = heapifyDown(pCurrentIndex);
	newIndex = heapifyUp(newIndex);	
}


inline int GridRunner::childLeft(int index){
	return (index+1)*2-1;
}
inline int GridRunner::childRight(int index){
	return (index+1)*2;
}
inline int GridRunner::parent(int index){
	return (int)(floor((double)(index-1)/2));
}